#include "gazebosim.h"
#include <string>

GazeboSim::GazeboSim()
{
    link_states_sub = nh.subscribe<gazebo_msgs::LinkStates>("/gazebo/link_states", 1, &GazeboSim::linkStatesCallBack, this);
    model_state_pub = nh.advertise<gazebo_msgs::ModelState>("/gazebo/set_model_state", 5);
    odom_pub = nh.advertise<nav_msgs::Odometry>("/odom", 10);
    cameraState.model_name = "camera";
    lidarState.model_name = "lidar";
    robotState.model_name = "robot";
}

void GazeboSim::linkStatesCallBack(const gazebo_msgs::LinkStatesConstPtr &msg)
{
    linkstate_data = *msg;
    odomTrans.frame_id_ = "odom";
    odomTrans.child_frame_id_ = "base_link";
    int base_fp_index;
    std::string base_fp_str("base_origin");
    for (base_fp_index = 0; base_fp_index < msg->name.size(); ++base_fp_index)
    {
        if (msg->name[base_fp_index].find(base_fp_str, msg->name[base_fp_index].length() - 12) != std::string::npos)
        {
            break;
        }
    }
    if (base_fp_index >= msg->name.size())
    {
        ROS_ERROR("Couldn't Find base_link");
        ros::shutdown();
    }
    odom_data.pose.pose = msg->pose[base_fp_index];
    odom_data.twist.twist = msg->twist[base_fp_index];

    ros::Time odomTime = ros::Time::now();
    odomTrans.frame_id_ = "odom";
    odomTrans.child_frame_id_ = "base_link";
    odomTrans.stamp_ = odomTime;
    geometry_msgs::Quaternion geoQuat = odom_data.pose.pose.orientation;
    odomTrans.setRotation(tf::Quaternion(geoQuat.x, geoQuat.y, geoQuat.z, geoQuat.w));
    odomTrans.setOrigin(tf::Vector3(odom_data.pose.pose.position.x, odom_data.pose.pose.position.y, 0.6));
    tfBroadcaster.sendTransform(odomTrans);

    odom_pub.publish(odom_data);

    cameraState.pose.orientation = geoQuat;
    cameraState.pose.position.x = odom_data.pose.pose.position.x;
    cameraState.pose.position.y = odom_data.pose.pose.position.y;
    cameraState.pose.position.z = 0.6;
    ROS_WARN("%f--%f--%f--   ", odom_data.pose.pose.position.x, odom_data.pose.pose.position.y, 0.6);
    model_state_pub.publish(cameraState);

    robotState.pose.orientation = geoQuat;
    robotState.pose.position.x = odom_data.pose.pose.position.x;
    robotState.pose.position.y = odom_data.pose.pose.position.y;
    robotState.pose.position.z = 0.6;
    model_state_pub.publish(robotState);

    lidarState.pose.orientation = geoQuat;
    lidarState.pose.position.x = odom_data.pose.pose.position.x;
    lidarState.pose.position.y = odom_data.pose.pose.position.y;
    lidarState.pose.position.z = 0.6;
    model_state_pub.publish(lidarState);
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "gazebosim");
    GazeboSim bp;
    ros::spin();
    return 0;
}